import numpy as np
from spatialmath import SO3


class Transformation:

    @staticmethod
    def eul2Rot(roll, pitch, yaw) -> np.ndarray:
        Rz = np.array([
            [np.cos(yaw), -np.sin(yaw), 0],
            [np.sin(yaw), np.cos(yaw), 0],
            [0, 0, 1]
        ])

        Ry = np.array([
            [np.cos(pitch), 0, np.sin(pitch)],
            [0, 1, 0],
            [-np.sin(pitch), 0, np.cos(pitch)]
        ])

        Rx = np.array([
            [1.0, 0.0, 0.0],
            [0, np.cos(roll), -np.sin(roll)],
            [0, np.sin(roll), np.cos(roll)]
        ])

        return Rz @ Ry @ Rx

    @staticmethod
    def eul2quat(roll, pitch, yaw) -> np.ndarray:
        R: SO3 = SO3.RPY(roll, pitch, yaw)
        resQuat = R.UnitQuaternion()
        return resQuat.A

    @staticmethod
    def Rz3(theta) -> np.ndarray:
        M = np.array([
            [np.cos(theta), -np.sin(theta), 0.0],
            [np.sin(theta), np.cos(theta), 0.0],
            [0, 0, 1]
        ])
        return M

    @staticmethod
    def CrossProduct_A(A: np.ndarray):
        M = np.array([
            [0.0, -A[2], A[1]],
            [A[2], 0.0, -A[0]],
            [-A[1], A[0], 0.0]
        ])
        return M

    @staticmethod
    def pseudoInv_right_weighted(M: np.ndarray, W: np.ndarray) -> np.ndarray:
        Mres: np.ndarray = M @ np.linalg.inv(W) @ M.T
        Mres = np.linalg.inv(W) @ M.T @ np.linalg.pinv(Mres)
        return Mres

    @staticmethod
    def dyn_pseudoInv(M: np.ndarray, dyn_M: np.ndarray, isMinv: bool) -> np.ndarray:
        Minv = np.zeros(0)
        if isMinv:
            Minv = dyn_M.copy()
        else:
            Minv = np.linalg.inv(dyn_M)

        temp = M @ Minv @ M.T
        res = Minv @ M.T @ np.linalg.pinv(temp)
        return res

    @staticmethod
    def diffRot(Rcur: np.ndarray, Rdes: np.ndarray):
        R = Rcur.T @ Rdes
        w = np.zeros(3)

        if Transformation.isDiagonal(R, 1e-5) and np.fabs(R[0, 0]) + np.fabs(R[1, 1]) + np.fabs(R[2, 2]) - 3 < 1e-3:
            w = np.zeros(3)
        elif Transformation.isDiagonal(R, 1e-5):
            w[:3] = [R[0, 0] + 1, R[1, 1] + 1, R[2, 2] + 1]
            w = w * np.pi / 2.0
        else:
            l = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
            sita = np.arctan2(np.linalg.norm(l), R[0, 0] + R[1, 1] + R[2, 2] - 1)
            w = sita * l / np.linalg.norm(l)
        w = Rcur @ w
        return w

    @staticmethod
    def isDiagonal(M: np.ndarray, prec: float) -> bool:
        is_diagonal = np.all(np.abs(M - np.diag(np.diagonal(M))) < prec)
        return is_diagonal
